#include "main.h"
#include "cmsis_os.h"
#include "stepmotor.h"
#include "point.h"
#include "stdlib.h"
#include "connect.h"

StepMotor motorX;
StepMotor motorY;
StepMotor motorZ;

MotorState motorZ_state = IDLE;
MotorState motorXY_state = IDLE;

extern int FLAG;

void StepMotor_Init(StepMotor* motor,
                    GPIO_TypeDef* DIR_Port, uint16_t DIR_Pin,
                    GPIO_TypeDef* PUL_Port, uint16_t PUL_Pin)
{
    motor->DIR_Port = DIR_Port;
    motor->DIR_Pin = DIR_Pin;
    motor->PUL_Port = PUL_Port;
    motor->PUL_Pin = PUL_Pin;
    motor->Speed = 0;
	motor->Pulse = 0;		
}


void StepMotor_Stop(StepMotor* motor) {motor->Speed = 0;motor->Pulse = 0;}


void StepMotor_Speed(StepMotor* motor)
{
	int speed = motor->Speed;

	if (speed == 0)
		return;
    else if (speed > 0 )
		HAL_GPIO_WritePin(motor->DIR_Port, motor->DIR_Pin, GPIO_PIN_RESET);
	else
		HAL_GPIO_WritePin(motor->DIR_Port, motor->DIR_Pin, GPIO_PIN_SET);
	
	int abs_speed = (speed >= 0) ? speed : -speed;
	int delay = 1000 / abs_speed;
	
	HAL_GPIO_TogglePin(motor->PUL_Port, motor->PUL_Pin);
		
	osDelay(delay);

}


void StepMotor_Pluse(StepMotor* motor)
{
	int pulse = motor->Pulse;
	
	if (motor->Pulse < 3 && motor->Pulse > -3)
		return;
    else if (motor->Pulse > 0 )
	{
		(motor->Pulse)--;
		HAL_GPIO_WritePin(motor->DIR_Port, motor->DIR_Pin, GPIO_PIN_RESET);
	}
	else
	{
		(motor->Pulse)++;
		HAL_GPIO_WritePin(motor->DIR_Port, motor->DIR_Pin, GPIO_PIN_SET);
	}
	
	HAL_GPIO_TogglePin(motor->PUL_Port, motor->PUL_Pin);
		
	//osDelay(2);
}


void GoToPoint(Point *current, Point *target)
{
	//ZVE10M2
//	motorX.Pulse = -(target->x - current->x);
//	motorY.Pulse = -(target->y - current->y);
	
	//USBCAMERA
	motorX.Pulse = (target->x - current->x);
	motorY.Pulse = (target->y - current->y);
}

int OnThePoint(Point *current, Point *target)
{
	int threshold = 3;
	//int threshold = 8;
	int dx = target->x - current->x;
	int dy = target->y - current->y;

	return (abs(dx) <= threshold && abs(dy) <= threshold);
}

void SetPulse(StepMotor *motor, int pulse)
{
	motor->Pulse = pulse;
}


void GetPiece(void)
{
	if (motorZ.Pulse < 3 && motorZ.Pulse > -3 && motorZ_state == IDLE)
	{
		motorZ.Pulse = 800;
		
		motorZ_state = DOWN;
	}
	else if (motorZ.Pulse < 5 && motorZ.Pulse > 0 && motorZ_state == DOWN)
	{
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_SET);
		motorZ.Pulse = -800;
		motorZ_state = UP;
	}
	else if (motorZ.Pulse < 0 && motorZ.Pulse > -3 && motorZ_state == UP)
	{
		motorZ_state = READY;
	}
}


void PutPiece(void)
{
	if (motorZ.Pulse < 3 && motorZ.Pulse > -3 && motorZ_state == READY)
	{
		motorZ.Pulse = 800;
		motorZ_state = DOWN;
	}
	else if (motorZ.Pulse < 5 && motorZ.Pulse > 0 && motorZ_state == DOWN)
	{
		motorZ.Pulse = -800;
		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET);
		motorZ_state = UP;
	}
	else if (motorZ.Pulse < 0 && motorZ.Pulse > -3 && motorZ_state == UP)
	{
		motorZ_state = OK;
		motorXY_state = OK;
	}
}

void PlacePiece(Point *motor, Point *piece, Point *target)
{
	if (motorXY_state == IDLE)
	{
		motorXY_state = GOPIECE;
		target->type = piece->type;
	}
	else if (motorXY_state == GOPIECE && motorZ_state != READY)
	{
		GoToPoint(motor, piece);
		if (OnThePoint(motor, piece))
			GetPiece();
	}
	else if (motorXY_state == GOPIECE && motorZ_state == READY)
	{	
		motorXY_state = GOBOARD;
	}
	else if (motorXY_state == GOBOARD && motorZ_state != OK)
	{
		GoToPoint(motor, target);
		if (OnThePoint(motor, target))
			PutPiece();
	}
	else if (motorZ_state == OK && motorXY_state == GOBOARD)
	{
		motorXY_state = OK;
		target->type = piece->type;
	}
}


void SetOrigin(Point *current, Point *origin)
{
	GoToPoint(current, origin);
	if (OnThePoint(current, origin) && motorXY_state == OK && motorZ_state == OK)
	{
		
		motorXY_state = IDLE; motorZ_state = IDLE;
	}
}

